#ifndef _USER_PID_H
#define _USER_PID_H 1

#include "stm32f10x.h"
#include "board_types.h"
#include "user_api_types.h"

#define MAX_ANGLE_INCLINATION 1000


enum
{
  Pitch = 0,
  Roll  = 1,
  Yaw   = 2
};

typedef struct
{
  int32_t p_gain;          /* proportional gain to keep angle level. */
  int32_t p_dyn_gain;      /* proportional dynamic gain for angle speed. */

  int32_t i_gain;          /* integral gain to keep angle level. */
  int32_t i_dyn_gain;      /* integral dynamic gain for angle speed. */
  int32_t i_state;         /* Integrator state,should be initialized to 0. */
  int32_t i_max;	         /* Maximum allowable integrator state. */
  int32_t i_min;           /* Minimum allowable integrator state. */

  int32_t d_gain;          /* derivative gain to keep angle level. */
  int32_t d_dyn_gain;      /* derivative dynamic gain for angle speed.*/
  int32_t d_state;         /* should be initialized by 0. */
  int32_t d_0_state;       /* should be initialized by 0. */
  int32_t d_1_state;       /* should be initialized by 0. */
  int32_t d_old_gyro_rate; /* for calculation delta angle speed, should be initialized by 0.*/
} PID_element;

extern PID_element PID[3];

void init_PID(void);
void init_PID_variable(PID_element *pid);

/* Pseudo derivative filter. */
int32_t update_PDF(
                    PID_element *PID,
                    int32_t RC_position,   /* +-900 for +-500microSec from RC channel. */
                    int32_t AnglePosition, /* +-1800 for +-180 degree from Kalman filter. */
                    int32_t Gyro_Rate      /* +-24000 for +- 300deg/sec. */
                );

int32_t constrain(int32_t x,int32_t y,int32_t z);




#endif